#include "../include/bag_dealer/SensorsFuser.h"

static size_t camera_idx = 0;

SensorsFuser::SensorsFuser() {
    frontPub = nh_.advertise<sensor_msgs::Image>("/synced/usb_cam0/image_raw", 10);
    leftPub = nh_.advertise<sensor_msgs::Image>("/synced/usb_cam1/image_raw", 10);
    rightPub = nh_.advertise<sensor_msgs::Image>("/synced/usb_cam2/image_raw", 10);
    lidarPub = nh_.advertise<sensor_msgs::PointCloud2>("/synced/velodyne_points", 10);
//deal message_filters
    message_filters::Subscriber<sensor_msgs::Image> front_img_sub(nh_, "/usb_cam0/image_raw", 10);
    message_filters::Subscriber<sensor_msgs::Image> left_img_sub(nh_, "/usb_cam1/image_raw", 10);
    message_filters::Subscriber<sensor_msgs::Image> right_img_sub(nh_, "/usb_cam2/image_raw", 10);
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh_, "/velodyne_points", 10);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
                                                    sensor_msgs::Image,
                                                    sensor_msgs::Image,
                                                    sensor_msgs::PointCloud2> myPolicy;
    message_filters::Synchronizer<myPolicy> synchronizer(myPolicy(30),
                                                         front_img_sub,
                                                         left_img_sub,
                                                         right_img_sub,
                                                         lidar_sub);
    synchronizer.registerCallback(boost::bind(&SensorsFuser::callback, this, _1, _2, _3, _4));
    ros::spin();
}

SensorsFuser::~SensorsFuser() {

}

void SensorsFuser::callback(const sensor_msgs::ImageConstPtr &img_front_msg, const sensor_msgs::ImageConstPtr &img_left_msg,
                       const sensor_msgs::ImageConstPtr &img_right_msg, const sensor_msgs::PointCloud2ConstPtr &pc_msg) {
    frontPub.publish(*img_front_msg);
//    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img_front_msg, img_front_msg->encoding);
//    auto img_cv = cv_ptr->image;
//    cv::cvtColor(img_cv, img_cv, CV_RGB2BGR);
//    cv::imwrite("/home/jackal/Downloads/data/1.jpg", img_cv);
    leftPub.publish(*img_left_msg);
    rightPub.publish(*img_right_msg);
    lidarPub.publish(*pc_msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sensor_fuser");
    SensorsFuser fuser;


    return 0;
}